﻿/*************************************************************************
 *     This file & class is part of the Object-Oriented Optimization
 *     Toolbox (or OOOT) Project
 *     Copyright 2010 Matthew Ira Campbell, PhD.
 *
 *     OOOT is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *  
 *     OOOT is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU General Public License for more details.
 *  
 *     You should have received a copy of the GNU General Public License
 *     along with OOOT.  If not, see <http://www.gnu.org/licenses/>.
 *     
 *     Please find further details and contact information on OOOT
 *     at http://ooot.codeplex.com/.
 *************************************************************************/
using System;
using OptimizationToolbox;
using StarMathLib;

namespace GearSynth.Evaluation
{
    internal class boundingboxConstraint : IInequality
    {
        private readonly GearTrainAnalysis GTAnalysis;
        private readonly double[] limits;

        internal boundingboxConstraint(GearTrainAnalysis GTAnalysis, userDefinedGoals udg)
        {
            this.GTAnalysis = GTAnalysis;
            limits = new[] { udg.minX, udg.maxX, udg.minY, udg.maxY, udg.minZ, udg.maxZ };
        }


        #region Implementation of IOptFunction

        public double calculate(double[] x)
        {
            var p = double.NegativeInfinity;
            for (int i = 0; i < GTAnalysis.numGears; i++)
            {
                var normal = new[]{GTAnalysis.positions[i][0, 2],
                    GTAnalysis.positions[i][1, 2],
                    GTAnalysis.positions[i][2, 2]
                };
                normal = StarMath.normalize(normal);

                var center = new[]{GTAnalysis.positions[i][0, 3],
                    GTAnalysis.positions[i][1, 3],
                    GTAnalysis.positions[i][2, 3]
                };
                var F = x[i * 4 + 2];
                var radius = GTAnalysis.diameters[i] / 2;
                var limitI = 0;
                for (int j = 0; j < 3; j++)
                    for (int k = -1; k < 2; k = k + 2)
                    {
                        var wallVector = new int[3];
                        wallVector[j] = k;
                        var wall = limits[limitI++];
                        var c = (double[])center.Clone();
                        var theta = Math.Acos(StarMath.multiplyDot(wallVector, normal));
                        theta = Math.PI / 2 - theta;
                        if (theta < 0)
                            c = StarMath.add(c, StarMath.multiply(-1.0 * F, normal));
                        c = StarMath.add(c, StarMath.multiply(radius * Math.Cos(theta), wallVector));

                        p = Math.Max(p, wall - c[j]);
                    }
            }
            return p;
        }

        #endregion
    }
}
